#include "mainwindow.h"
#include "ui_mainwindow.h"
#include "ecat_manager/ecat_manager.h"
#include "ecat_manager/servo_setting.h"

// format an rvalue, return a QString instance (not still an rvalue)
// but according to CXX RVO(Return Value Optimization)
// str is going to be moved to the return of the function
QString format_binary_text(QString&& str, int to_length = sizeof(short)*8) {
    int size = str.size();

    while (size > to_length) to_length += 4;

    str.reserve(to_length);

    while (str.size() < to_length) {
        str.push_front('0');
    }

    int insert_pos = str.size();

    while (insert_pos > 4) {
        insert_pos -= 4;
        str.insert(insert_pos, "_");
        // --insert_pos;
    }

    return std::move(str);
}

MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent)
    , ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    t1_ = new QTimer(this);
    connect(t1_, &QTimer::timeout, this, [&](){

        if (req_connect_)
        {
//            ECAT->sync();
//            // SERVO->set_servo_enable(SERVO_NUM, true); // enable the last
//            SERVO->commit_setting_once();
            EcatSyncer syncer(ECAT);

            {
                ServoSetter setter(SERVO);
                int slave = ui->cbx_servocnt->currentIndex();
                if (slave >= 1)
                    SERVO->set_axis_target_value(slave, ui->spinBox->value());
            }

        }

    });

    connect(ui->pb_servoconnect, &QPushButton::clicked, this, [&](bool checked){
        req_connect_ = checked;
        if (!req_connect_) {

            // 终止连接
            for (int i = 1; i <= SERVO_NUM; ++i) {
                ECAT->servo_set_disable_operation(i);
            }

            ECAT->sync();

            QEventLoop el;
            QTimer::singleShot(2, &el, &QEventLoop::quit);
            el.exec();

            ECAT->exit_master();

            ui->cbx_servocnt->setEnabled(false);
            t2_->stop();

        } else {
            // 启动连接
//            int retry_times = 0;
//            while (!ECAT->init_slaves(IFNAME) ) {
//                ui->pb_servoconnect->setChecked(false); // 没连接上
//                req_connect_ = false;
//                ++retry_times; // 重试
//                if (retry_times > 10) {
//                    // 超过次数
//                    return;
//                }
//            }

//            // 连接成功
//            ui->pb_servoconnect->setChecked(true);
//            req_connect_ = true;

            if (!ECAT->try_init_slaves(IFNAME, 10)) {
                ui->pb_servoconnect->setChecked(false); // 没连接上
                req_connect_ = false;
                return;
            }


            // 连接之后获取当前位置，初始化驱动器的目标位置
            // 这一部分代码也可以在类内部做
            for (int i = 1; i <= SERVO_NUM; ++i) {
                position_actual_value_t act_pos = ECAT->servo_state_actual_pos(i);
                qDebug() << QString("servo %0 act pos is : %1").arg(i).arg(act_pos);

                ECAT->set_servo_target_position(i, act_pos);
            }

            ui->cbx_servocnt->setEnabled(true);
            t2_->start(20);
        }
    });


    connect(ui->pb_autoclearfault, &QPushButton::clicked, this, [&](bool checked){
        SERVO->set_servo_auto_clear_fault(checked);
    });

    t1_->start(2); // start timer

    // 下拉框添加驱动器选项
    for (int i = 1; i <= SERVO_NUM; ++i) {
        ui->cbx_servocnt->addItem(QString("驱动器%0").arg(i));
    }

    // 添加驱动器使能按钮
    for (int i = 1; i <= SERVO_NUM; ++i) {
        QPushButton* pb = new QPushButton(QString("驱动器%0使能").arg(i), this);
        pb->setCheckable(true);
        pb->setChecked(false);
        ui->verticalLayout_enable->addWidget(pb);
        connect(pb, &QPushButton::clicked, this, [&](bool checked){
            auto text = static_cast<QPushButton*>(sender())->text();
            qDebug() << text << checked;
            qDebug() << text.mid(3, 1);
            int servo_num = text.mid(3, 1).toInt();
            SERVO->set_servo_enable(servo_num, checked);
            if (!checked) {
                ui->spinBox->setValue(ECAT->servo_state_actual_pos(servo_num));
            }
        });
    }

    t2_ = new QTimer(this);
    connect(t2_, &QTimer::timeout, this, [&](){
        static EcatManager::servo_stat_t ss;

        int index = ui->cbx_servocnt->currentIndex();
        if (index == 0) {
            // no display
        } else {
            // show the indexed servo state
            ECAT->get_servo_state(index, ss);
            ui->le_actpos->setText(QString::number(ss.position_actual_value, ui->le_actpos_s->text().toInt()));
            ui->le_errorcode->setText(QString::number(ss.error_code, ui->le_errorcode_s->text().toInt()));
            ui->le_statusword->setText(format_binary_text(
                                           QString::number(ss.status_word, ui->le_statusword_s->text().toInt()), sizeof(status_word_t)*8
                                           ));
        }
    });


}

MainWindow::~MainWindow()
{
    qDebug() << "~~~";
    t1_->stop();

    // 终止连接
    for (int i = 1; i <= SERVO_NUM; ++i) {
        ECAT->servo_set_disable_operation(i);
    }

    ECAT->sync();

    QEventLoop el;
    QTimer::singleShot(2, &el, &QEventLoop::quit);
    el.exec();

    ECAT->exit_master();
    delete ui;
}

void MainWindow::closeEvent(QCloseEvent *event)
{
    qDebug() << "111";
    QMainWindow::closeEvent(event);
}


void MainWindow::on_pb_sendcontrol_clicked()
{
    static EcatManager::servo_ctrl_t cc;

    int index = ui->cbx_servocnt->currentIndex();
    bool ok = false;
    if (index == 0) {
        // no ctrl
    } else {
        // change the indexed servo control
        cc.control_word = ui->le_controlword->text().replace("_", "").replace(" ","").toUShort(&ok, ui->le_controlword_s->text().toUInt());
        if (!ok) return;
        cc.modes_of_operation = ui->le_opmode->text().toInt(&ok, ui->le_opmode_s->text().toUInt());
        if (!ok) return;
        cc.target_position = ui->le_targetpos->text().toInt(&ok, ui->le_targetpos_s->text().toUInt());
        if (!ok) return;

        ECAT->set_servo_ctrl(index, cc);
    }
}

void MainWindow::on_cbx_servocnt_currentIndexChanged(int index)
{
    if (index >= 1)
        ui->spinBox->setValue(ECAT->servo_state_actual_pos(index));
}

void MainWindow::on_spinBox_v_valueChanged(int arg1)
{
    ui->spinBox->setSingleStep(arg1);
}
